/*
 *
 * Copyright (C) 2014
 * Julio Jarquin, Gustavo Arechavaleta <garechav@cinvestav.edu.mx>
 * CINVESTAV - Saltillo Campus
 *
 * This file is part of HRLocomotion-1.0
 * HRLocomotion-1.0 free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * HRLocomotion-1.0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301  USA
 */


/**
 *	\file src/mpc_locomotion.h
 *	\author Julio Jarquin, Gustavo Arechavaleta
 *	\version 1.0
 *	\date 2015
 *
 *	Declaration of the Motion class designed to implement the model predictive control scheme proposed by A. Herdt, P.-B. Wieber, et. al. 2011 .
 */

#ifndef HR_LOCOMOTION_MOTION_H
#define HR_LOCOMOTION_MOTION_H

#include <vector>
#include "Eigen/Dense"
#include "hr_types.h"
#include <boost/shared_ptr.hpp>
namespace qpOASES{   //forward declaration of qpOASES::SQProblem class
    class SQProblem;
}

namespace hrLocomotion{

class MPCLocomotion{

    public:
        MPCLocomotion();
        ~MPCLocomotion();

        ///Generic methods
        bool init();
       // template<typename T> Eigen::Matrix<T,Eigen::Dynamic, 1> getU(); //default templates not working here :(
        VectorXr getU();
        void setInitialPosture(real_t x, real_t y);
        void solveNext();

    protected:

    private:
        ///MPC Constants

        const real_t g;      /* Gravity constant */
        real_t h;            /* CoM fixed heigh */
        real_t alfa;         /* Jerk gain */
        real_t beta;         /* Velocity gain */
        real_t gamma;        /* ZMP gain */
        real_t Td;           /* Double support period */
        unsigned int m;      /* Future m steps to be considered */
        unsigned int Ss;     /* Support stage, simple + double, in periods Td */
        unsigned int N;      /* Previewable window in periods Td */


        bool actual;         /* Current foot on ground, 1 left, 0 right */

        MatrixXr Pvs;        /* Mtrices P of lenght N, are the recursivity of dynamics,  */
        MatrixXr Pvu;
        MatrixXr Pzs;
        MatrixXr Pzu;
        std::vector<VectorXr> Uc;    /* Container of selection vectors Uc, current ZMP reference */
        std::vector<MatrixXr> Ur;    /* Container of selection matrices Ur, next ZMP reference */
        std::vector<MatrixXr> Q;     /* Container of matrices Q, ------ */



        ///MPC Needed vars
        VectorXr pk;         /* vector of N size, ------ */
        real_t xfc;          /* Current X position of foot on ground */
        real_t yfc;          /* Current Y position of foot on ground */
        VectorXr x_ac;       /* Actual state in X */
        VectorXr y_ac;       /* Actual state in Y */
        real_t xpref;        /* Reference velocity in X */
        real_t ypref;        /* Reference velocity in Y */
        VectorXr ones;       /* Vector of N size, to form velocity reference window */


        VectorXr x_next;
        VectorXr y_next;
        VectorXr B_din;      /* Vector B in discrete dynamics */

        VectorXr uk;         /* Control vector of 2*(N+m) size to be minimized */
        MatrixXr A_din;      /* Matrix A in discrete dynamics */
        //    RowVectorXd C_din;   /* Matrix for ZMP aproxiamtion, cart-table model */


        //QP Objects and Paramters
        //qpOASES::SQProblem * quadprog;     /* QP solver objet of qpOASES library */
        boost::shared_ptr<qpOASES::SQProblem> quadprog;
        int nWSR;               /* QP parameter, maximum number of working set recalculations to be performed */
        int nV;                 /* Number of variables of QP*/
        int nC;                 /* Number of constrains of QP*/
        bool walking;           /* Flag when walk is active */
        int stage;              /* Cycle stage */


        // Gait parameters
        real_t maxStepX;        /* Maximum forward translation along X */
        real_t minStepX;        /* Maximum backward translation along X */
        real_t maxStepY;        /* Maximum lateral translation along Y */
        real_t minFootSep;      /* Minimum distance between both feet along Y */
        real_t minStepY;        /* Minimum distance considering foot orientation along Y */

        // Restriction matrices
        MatrixXr A_;            /* Complete constraint matrix (ZMP and position) */
        VectorXr b_;            /* Complete uper bound (ZMP and position) */

        std::vector<MatrixXr> A_z;   /* Constraint matrix for ZMP for each stage */
        MatrixXr A_p;           /* Constraint matrix for foot position */
        MatrixXr A_pl;          /* Constraint matrix for foot position with left on ground */
        MatrixXr A_pr;          /* Constraint matrix for foot position with right on ground*/
        VectorXr b_zaux;        /* Uper bound for ZMP including Xfc and Yfc */
        VectorXr b_z;           /* Uper bound for ZMP */
        VectorXr b_paux;        /* Uper bound for foot position */
        VectorXr b_p;           /* Uper bound for foot position including Xfc and Yfc */

        MatrixXr D_z;           /* Selection matrix for margin */
        MatrixXr points;        /* Points that define de bounding box of the rotating foot */
        MatrixXr rpoints;       /* Rotated points of the bounding box given an orientation */
        MatrixXr R;             /* Rotation matrix along Z axis */
        real_t theta;           /* Final orientation for the next step in relation with the actual */
        real_t margin;          /* Security distance to the edge of foot */



        //Ugly initialization methods
        void initMatricesP();    //Needed by Q and Pk
        void initMatricesU();    //Needed by Q and Pk
        void initMatricesQ();    //Q for QP Hessian Matrix
        void setVectorPk(int i); //Pk from QP F matrix
        void initRestrictions(); //Foot position
        void setRestrictions();  //Foot Rotation




};




}




#endif // HR_LOCOMOTION_MOTION_H

